/*
 * Copyright The Slam Authors
 */

#ifndef SLAM_MAPPING_LOCALIZATION_EVALUATOR_H_
#define SLAM_MAPPING_LOCALIZATION_EVALUATOR_H_

#include <map>
#include <string>

#include "slam/mapping/id.h"
#include "slam/mapping/pose_graph_data.h"
#include "slam/mapping/proto/pose_graph_options.pb.h"

namespace slam {
namespace mapping {

class SlamEvaluator {
 public:
  SlamEvaluator();
  SlamEvaluator(const proto::PoseGraphOptions::SlamEvaluatorOptions& options);

  void AddNode(const NodeId& node_id, const transform::Rigid2d& local_pose);

  void AddConstraint(
      const std::vector<PoseGraphInterface::Constraint>& constraints);

  bool IsLocalizationSuccess() const;

  void Reset();

  void ResetTraveledDistance();

 private:
  const proto::PoseGraphOptions::SlamEvaluatorOptions options_;
  NodeId last_connect_node_id_;
  bool has_last_node_ = false;
  bool has_global_constraint_ever_ = false;
  NodeId last_node_id_;
  transform::Rigid2d last_node_pose_;
  double distance_traveled_from_last_connect_ = 0;
  double angle_traveled_from_last_connect_ = 0;
  volatile bool is_localization_success_ = false;
};

}  // namespace mapping
}  // namespace slam

#endif
